import sim

def connect_to_coppeliasim(port=19999):
    sim.simxFinish(-1)  # 关闭所有可能存在的连接
    clientID = sim.simxStart('127.0.0.1', port, True, True, 5000, 5)
    if clientID != -1:
        print('Connected to CoppeliaSim')
    else:
        raise Exception('Failed to connect to CoppeliaSim')
    return clientID

def get_joint_angle(clientID, joint_name):
    # 获取关节句柄
    _, jointHandle = sim.simxGetObjectHandle(clientID, joint_name, sim.simx_opmode_blocking)
    
    # 获取关节当前角度（以弧度为单位）
    _, angle = sim.simxGetJointPosition(clientID, jointHandle, sim.simx_opmode_blocking)
    
    # 将弧度转换为角度
    angle_deg = angle * (180 / 3.14159265)
    
    return angle_deg

def main():
    clientID = connect_to_coppeliasim()
    print(clientID)
    
    try:
        angle = get_joint_angle(clientID, 'joint3')
        print(f'The current angle of joint3 is: {angle:.2f} degrees')
    finally:
        sim.simxFinish(clientID)  # 关闭连接

if __name__ == '__main__':
    main()
